#include "scanContextDescriptor.h"

scan_context_descriptor::scan_context_descriptor(
	int ring_num 				= 20,    //（环数）
	int sector_num 				= 60, // （扇区数）
	int candidates_num 			= 6, //（候选点数）
	double distance_threshold 	= 0.14, //（距离阈值）
	double lidar_height 		= 1.65, //（激光雷达高度）
	double max_radius 			= 80.0, //（最大半径）
	int exclude_recent_num 		= 100, //（最近排除数）
	int robot_num				= 1, //（机器人数量）
	int id						= 0 ) : //（机器人id）
pc_ring_num_(ring_num), // 20 in the original paper (IROS 18)
pc_sector_num_(sector_num), // 60 in the original paper (IROS 18)
candidates_num_(candidates_num),// 10 is enough. (refer the IROS 18 paper)
distance_threshold_(distance_threshold), // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <,
							// DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness).
							// 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
lidar_height_(lidar_height), // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord)
							// if you use robot-coord-transformed lidar scans, just set this as 0.
pc_max_radius_(max_radius), // 80 meter max in the original paper (IROS 18)
exclude_recent_num_(exclude_recent_num), // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. 
robot_num_(robot_num), // number of robot in robotic swarm
id_(id) // this robot id
{
	// allocate memory 构造函数为每个机器人分配内存，以存储极坐标上下文、关键点和索引映射
	for(int i = 0; i < robot_num_; i++)
	{
		std::vector<Eigen::MatrixXd> base_matrix;
		Eigen::MatrixXf base_vector;
		std::vector<int> base_int;
		polarcontexts.push_back(base_matrix);
		polarcontext_ringkeys.push_back(base_vector);
		indexes_maps.push_back(base_int);
	}
}

scan_context_descriptor::~scan_context_descriptor()
{
	
}

/*** scan context param-independent helper functions ***/
//定义一个名为xy2theta的函数，用于将笛卡尔坐标系中的点（_x，_y）转换为相应的极坐标角度值
float scan_context_descriptor::xy2theta(
	float & _x,
	float & _y)
{
	// first quadrant
	if((_x >= 0) & (_y >= 0))
	{
		return (180/M_PI) * atan(_y / _x);
	}
	// second quadrant
	if((_x < 0) & (_y >= 0))
	{
		return 180 - ((180/M_PI) * atan(_y / (-_x)));
	}
	// third quadrant
	if((_x < 0) & (_y < 0))
	{
		return 180 + ((180/M_PI) * atan(_y / _x));
	}
	// forth quadrant
	if((_x >= 0) & (_y < 0))
	{
		return 360 - ((180/M_PI) * atan((-_y) / _x));
	}
}

// 一个名为circshift的函数，用于将矩阵中的列向右移动
// 两个参数一个名为_mat的Eigen矩阵和一个整数_num_shift，表示要向右移动的列数
Eigen::MatrixXd scan_context_descriptor::circshift(
	const Eigen::MatrixXd &_mat,
	int _num_shift)
{
	// shift columns to right direction 
	assert(_num_shift >= 0);  // assert关键字用来确保_num_shift大于零保证向右平移

    //如果_num_shift等于0，则函数直接返回输入的矩阵_mat
	if(_num_shift == 0)
	{
		Eigen::MatrixXd shifted_mat( _mat );
		return shifted_mat; // Early return 
	}

    // 如果_num_shift大于0，则函数会创建一个名为shifted_mat零矩阵函数，使用循环遍历_mat的每一列，
    // 并将其移动_num_shift个位置，即将原来的第col_idx列移动到新矩阵的第new_location列
    // 函数返回移动后的新矩阵shifted_mat
	Eigen::MatrixXd shifted_mat = Eigen::MatrixXd::Zero(_mat.rows(), _mat.cols());
	for(int col_idx = 0; col_idx < _mat.cols(); col_idx++)
	{
		int new_location = (col_idx + _num_shift) % _mat.cols();
		shifted_mat.col(new_location) = _mat.col(col_idx);
	}

	return shifted_mat;
}

// 定义一个名为eig2stdvec的函数，用于将Eigen矩阵转换为std::vector<float>类型的向量
// 接受一个名为_eigmat的Eigen矩阵作为输入参数
std::vector<float> scan_context_descriptor::eig2stdvec(
	Eigen::MatrixXd _eigmat)
{
    // 首先通过_eigmat.data()获取矩阵的数据指针，然后使用_eigmat.size()获取矩阵中元素的总数。
    // 接着，将这些数据指针和元素总数作为参数，创建一个std::vector<float>类型的向量vec
	std::vector<float> vec(_eigmat.data(), _eigmat.data() + _eigmat.size());
	return vec;
}

/*** scan context functions ***/
// 定义一个名为makeScancontext的函数，用于生成Scan Context描述符
// 函数接受一个名为scan_down的pcl::PointCloudpcl::PointXYZI类型的点云数据作为输入，
// 以及一个指向std::vector<float>类型的指针data，用于存储描述符数据
Eigen::MatrixXd scan_context_descriptor::makeScancontext(
	const pcl::PointCloud<pcl::PointXYZI>& scan_down,
	std::vector<float>* data)
{
	// TicToc t_making_desc;

    //获取点云中的点数目num_pts_scan_down
	int num_pts_scan_down = scan_down.points.size();

	// main
	const int NO_POINT = -1000;
	Eigen::MatrixXd desc = NO_POINT * Eigen::MatrixXd::Ones(pc_ring_num_, pc_sector_num_);
    //ring_num_(环数)20    sector_num_(扇数)60

	pcl::PointXYZI pt;
	float azim_angle, azim_range; // wihtin 2d plane (角度和距离)
	int ring_idx, sctor_idx;
    // 使用循环变量pt_idx遍历num_pts_scan_down个点，
    // 从scan_down点云数据中获取每个点的x、y、z坐标，并将z坐标加上lidar_height_，得到相对于激光雷达的高度值
	for(int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
	{
		pt.x = scan_down.points[pt_idx].x; 
		pt.y = scan_down.points[pt_idx].y;
		pt.z = scan_down.points[pt_idx].z + lidar_height_; // naive adding is ok (all points should be > 0).

		// xyz to ring, sector
		azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);//距离
		azim_angle = xy2theta(pt.x, pt.y); // 角度

		// if range is out of roi, pass
		if(azim_range > pc_max_radius_)
		{
			continue;
		}

        // 根据点的距离azim_range和角度azim_angle，计算出该点对应的环数ring_idx和扇区数sctor_idx。
        // 这里使用了std::min和std::max函数来确保环数和扇区数在合理范围内，并使用ceil函数对计算结果进行取整。
		ring_idx = std::max(std::min(pc_ring_num_, int(ceil((azim_range / pc_max_radius_) * pc_ring_num_))), 1);
		sctor_idx = std::max(std::min(pc_sector_num_, int(ceil((azim_angle / 360.0) * pc_sector_num_))), 1);

		// taking maximum z
        // 更新Scan Context描述符矩阵中每个环和扇区的值，保留最大的高度值作为描述符
		if(desc(ring_idx-1, sctor_idx-1) < pt.z) // -1 means cpp starts from 0
		{
			desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
		}
	}

	// reset no points to zero (for cosine dist later)
    // 指针data指向的std::vector<float>中，对描述符数据的提取和存储
	for(int row_idx = 0; row_idx < desc.rows(); row_idx++)
	{
		for(int col_idx = 0; col_idx < desc.cols(); col_idx++)
		{
			if(desc(row_idx, col_idx) == NO_POINT)
			{
				desc(row_idx, col_idx) = 0;
			}
			data->push_back(desc(row_idx, col_idx));
		}
	}

	// t_making_desc.toc("PolarContext making");
    // 循环结束后，desc矩阵中存储了点云中每个环和扇区的最大高度值，用于构建Scan Context描述符

	return desc;
}

// 通过计算每个环的描述符在每个扇区上的平均值，生成环向不变特征矩阵。
// 该特征矩阵可用于后续的相似性度量、分类或其他任务
Eigen::MatrixXf scan_context_descriptor::makeRingkeyFromScancontext(
	const Eigen::MatrixXd& desc)
{
	// summary: rowwise mean vector
	Eigen::MatrixXf invariant_key(desc.rows(), 1);
	for(int row_idx = 0; row_idx < desc.rows(); row_idx++)
	{
		Eigen::MatrixXd curr_row = desc.row(row_idx);
		invariant_key(row_idx, 0) = curr_row.mean();
	}

    // 生成的环向不变特征矩阵invariant_key   ？？？
	return invariant_key;
}

// 通过计算每个扇区上所有环的描述符的平均值，生成扇区向不变特征矩阵。
// 该特征矩阵可用于后续的相似性度量、分类或其他任务。
Eigen::MatrixXd scan_context_descriptor::makeSectorkeyFromScancontext(
	const Eigen::MatrixXd& desc)
{
	// summary: columnwise mean vector
	Eigen::MatrixXd variant_key(1, desc.cols());
	for(int col_idx = 0; col_idx < desc.cols(); col_idx++)
	{
		Eigen::MatrixXd curr_col = desc.col(col_idx);
		variant_key(0, col_idx) = curr_col.mean();
	}

    //生成的扇区向不变特征矩阵variant_key
	return variant_key;
}

//通过比较扇区向不变特征矩阵之间的差异范数，找到最佳的对齐位移值，以实现扇区之间的快速对齐
// fastAlignUsingVkey函数，用于使用扇区向不变特征进行快速对齐
// 两个Eigen::MatrixXd类型的扇区向不变特征矩阵vkey1和vkey2作为输入
int scan_context_descriptor::fastAlignUsingVkey(
	const Eigen::MatrixXd& vkey1,
	const Eigen::MatrixXd& vkey2)
{
	int argmin_vkey_shift = 0; // 表示扇区向不变特征对齐的最小位移
	double min_veky_diff_norm = 10000000; // 表示扇区向不变特征的最小差异范数
	for(int shift_idx = 0; shift_idx < vkey1.cols(); shift_idx++)
	{
        //利用上面定义的函数 circshift 将vkey2向右循环位移shift_idx个位置，得到位移后的矩阵vkey2_shifted
		Eigen::MatrixXd vkey2_shifted = circshift(vkey2, shift_idx);

        // 计算vkey1和vkey2_shifted之间的差异矩阵vkey_diff
		Eigen::MatrixXd vkey_diff = vkey1 - vkey2_shifted;

        //计算当前差异矩阵的范数cur_diff_norm，表示扇区向不变特征的差异程度
		double cur_diff_norm = vkey_diff.norm();
        // 如果当前差异范数小于最小差异范数min_veky_diff_norm，
        // 则更新argmin_vkey_shift为当前位移值shift_idx，
        // 同时更新min_veky_diff_norm为当前差异范数。
		if(cur_diff_norm < min_veky_diff_norm)
		{
			argmin_vkey_shift = shift_idx;
			min_veky_diff_norm = cur_diff_norm;
		}
	}

    // 返回最优的位移值argmin_vkey_shift
	return argmin_vkey_shift;
}

// scan_context_descriptor命名空间中的distDirectSC函数，用于计算两个Scan Context描述符之间的直接距离
// 接受两个Eigen::MatrixXd类型的Scan Context描述符矩阵sc1和sc2作为输入
double scan_context_descriptor::distDirectSC(
	const Eigen::MatrixXd& sc1,
	const Eigen::MatrixXd& sc2) // "d" (eq 5) in the original paper (IROS 18)
{
	int num_eff_cols = 0; // i.e., to exclude all-nonzero sector
    // 用于计算有效的扇区列数（即除去所有非零扇区）
	double sum_sector_similarity = 0;
    // 用于累加扇区相似度的和
	for(int col_idx = 0; col_idx < sc1.cols(); col_idx++)
	{
        //在每次迭代中，将sc1和sc2的对应列提取为Eigen::VectorXd类型的变量col_sc1和col_sc2
		Eigen::VectorXd col_sc1 = sc1.col(col_idx);
		Eigen::VectorXd col_sc2 = sc2.col(col_idx);

        //检查col_sc1和col_sc2的范数是否为0，
		if((col_sc1.norm() == 0) | (col_sc2.norm() == 0))
		{
			continue; // don't count this sector pair.
		}

        // 如果扇区不全为零，计算当前扇区对的相似度。相似度计算采用两个扇区对应列的点积除以它们的范数乘积
		double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());

        // 将当前扇区对的相似度累加到sum_sector_similarity中。
        // 同时，有效的扇区列数num_eff_cols加1
		sum_sector_similarity = sum_sector_similarity + sector_similarity;
		num_eff_cols = num_eff_cols + 1;
	}
	// 计算平均扇区相似度sc_sim
	double sc_sim = sum_sector_similarity / num_eff_cols;
    // 函数返回1减去平均扇区相似度sc_sim，即直接距离
	return 1.0 - sc_sim;
}

// 通过快速对齐和搜索最佳位移值，计算两个Scan Context描述符之间的距离
// 接受两个Eigen::MatrixXd类型的Scan Context描述符矩阵sc1和sc2作为输入
std::pair<double, int> scan_context_descriptor::distanceBtnScanContext(
	const Eigen::MatrixXd& sc1,
	const Eigen::MatrixXd& sc2) // "D" (eq 6) in the original paper (IROS 18)
{
	// 1. fast align using variant key (not in original IROS18)
    // 函数通过调用makeSectorkeyFromScancontext函数
    // 分别生成sc1和sc2的扇区向不变特征矩阵vkey_sc1和vkey_sc2
	Eigen::MatrixXd vkey_sc1 = makeSectorkeyFromScancontext(sc1);
	Eigen::MatrixXd vkey_sc2 = makeSectorkeyFromScancontext(sc2);
    // 使用fastAlignUsingVkey函数对vkey_sc1和vkey_sc2进行快速对齐，
    // 得到对齐的最佳位移值argmin_vkey_shift
	int argmin_vkey_shift = fastAlignUsingVkey(vkey_sc1, vkey_sc2);

	// int SEARCH_RADIUS = round(0.5 * SEARCH_RATIO * sc1.cols());
    // a half of search range
	int SEARCH_RADIUS = round(0.5 * 0.1 * sc1.cols());
    // a half of search range

    //该代码的作用是计算搜索范围shift_idx_search_space中的位移索引。
    // 将对齐的最佳位移值argmin_vkey_shift作为初始值加入其中，
    // 然后通过循环计算位移值加上和减去当前迭代值后取模得到的索引，
    // 并将它们加入shift_idx_search_space中。
    // 最后对shift_idx_search_space进行排序
	std::vector<int> shift_idx_search_space { argmin_vkey_shift };
	for(int ii = 1; ii < SEARCH_RADIUS + 1; ii++)
	{
		shift_idx_search_space.push_back((argmin_vkey_shift + ii + sc1.cols()) % sc1.cols());
		shift_idx_search_space.push_back((argmin_vkey_shift - ii + sc1.cols()) % sc1.cols());
	}
	std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());
    // 该代码的作用是对整型向量shift_idx_search_space进行排序，以便后续的搜索和计算

	// 2. fast columnwise diff
    // 用于在搜索范围内寻找最小的直接距离和对齐的最佳位移值
	int argmin_shift = 0;
	double min_sc_dist = 10000000;
	for(int num_shift: shift_idx_search_space)
	{
		Eigen::MatrixXd sc2_shifted = circshift(sc2, num_shift);
		double cur_sc_dist = distDirectSC(sc1, sc2_shifted);
		if(cur_sc_dist < min_sc_dist)
		{
			argmin_shift = num_shift;
			min_sc_dist = cur_sc_dist;
		}
	}

    //返回最小距离和对齐的最佳位移值。
	return std::make_pair(min_sc_dist, argmin_shift);
}

// User-side API
// 用于保存Scan Context描述符和对应的关键字
// 接受一个指向float类型的描述符矢量descriptor_vec，一个整型变量robot和一个整型变量index作为输入
void scan_context_descriptor::saveDescriptorAndKey(
	const float* descriptor_vec,
	const int8_t robot,
	const int index)
{
	// decode scan context
    //首先将描述符矢量解码得到描述符矩阵，然后调用save函数将描述符矩阵、机器人编号和索引保存。
	Eigen::MatrixXd sc = Eigen::MatrixXd::Ones(pc_ring_num_, pc_sector_num_);
	for(int row_idx = 0; row_idx < sc.rows(); row_idx++)
	{
		for(int col_idx = 0; col_idx < sc.cols(); col_idx++)
		{
            // 函数通过将描述符矢量descriptor_vec解码得到一个Eigen::MatrixXd类型的Scan Context描述符矩阵sc。
            // 该矩阵的大小为pc_ring_num_行pc_sector_num_列，其中每个元素都对应一个扇区的特征值
			sc(row_idx, col_idx) = descriptor_vec[row_idx*sc.cols()+col_idx];
		}
	}

	save(sc, robot, index);
    // 保存描述符矩阵、机器人编号和索引
}

//将 Scan Context 描述符和对应的关键字保存到类的成员变量中
void scan_context_descriptor::save(
	const Eigen::MatrixXd sc,
	const int8_t robot,
	const int index)
{
    // 调用 makeRingkeyFromScancontext 函数和 makeSectorkeyFromScancontext 函数，
    // 分别生成描述符矩阵对应的环形关键字矢量 ringkey 和扇形关键字矩阵 sectorkey
	Eigen::MatrixXf ringkey = makeRingkeyFromScancontext(sc);
	Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext(sc);

    //将描述符矩阵 sc 添加到类的成员变量 polarcontexts 中
	polarcontexts[robot].push_back(sc); // desc
    // 调整类的成员变量 polarcontext_ringkeys 的大小，
    // 使其具有 pc_ring_num_ 行和 polarcontexts[robot].size() 列，
    // 并将 ringkey 的第一列复制到 polarcontext_ringkeys 的最后一列中
	polarcontext_ringkeys[robot].conservativeResize(pc_ring_num_, polarcontexts[robot].size());
	polarcontext_ringkeys[robot].block(0, polarcontexts[robot].size()-1, pc_ring_num_, 1) = ringkey.block(0, 0, pc_ring_num_, 1);
    // 在成员变量 indexes_maps 中添加一个新的索引映射，
    // 将机器人编号 robot 和索引 index 存储为一个 std::pair 对象，
    // 并将其添加到 polarcontext_indexs 中。
	indexes_maps[robot].push_back(polarcontext_indexs.size());
    // polarcontext_indexs 是一个 std::vector 类型的容器，用于保存所有描述符矩阵的机器人编号和索引
	polarcontext_indexs.push_back(std::make_pair(robot, index)); // indexs
}


// scan_context_descriptor 命名空间中的 makeAndSaveDescriptorAndKey 函数，
// 用于生成并保存Scan Context描述符和关键字，并返回描述符数据
// 接受一个 pcl::PointCloud<pcl::PointXYZI> 类型的点云数据 scan，
// 一个 int8_t 类型的机器人编号 robot，
// 以及一个整型变量 index 作为输入
std::vector<float> scan_context_descriptor::makeAndSaveDescriptorAndKey(
	const pcl::PointCloud<pcl::PointXYZI>& scan,
	const int8_t robot,
	const int index)
{
	std::vector<float> descriptor_data; //用于存储生成的描述符数据
	Eigen::MatrixXd sc = makeScancontext(scan, &descriptor_data);
    // 将点云数据 scan 转换为 Scan Context 描述符矩阵，并将描述符数据存储在 descriptor_data 容器中
    // size:(pc_ring_num_, pc_sector_num_)
	save(sc, robot, index);
    // 将描述符矩阵 sc 和机器人编号 robot、索引 index 一起保存到类的成员变量中

	return descriptor_data;
}
// 是 scan_context_descriptor 命名空间中的 detectIntraLoopClosureID 函数的函数框架，
// 用于检测当前点云帧是否与之前的某个点云帧存在环回闭合
std::pair<int, float> scan_context_descriptor::detectIntraLoopClosureID(
	const int cur_ptr)
{

}

// 代码是 scan_context_descriptor 命名空间中的 detectInterLoopClosureID 函数，
// 用于检测当前点云帧与其他点云帧之间是否存在跨机器人的环回闭合，并返回检测结果
// 接受一个整型变量 cur_ptr，表示当前点云帧的索引
std::pair<int, float> scan_context_descriptor::detectInterLoopClosureID(
	const int cur_ptr) // int: nearest node index, float: relative yaw  
{
    // 获取当前点云帧所属的机器人编号 robot_id 和帧编号 frame_id，
    // 并从成员变量 polarcontext_ringkeys 和 polarcontexts 中
    // 获取当前点云帧对应的环形关键字矢量 ringkey 和扇形关键字矩阵 polarcontext
	std::pair<int, float> result {-1, 0.0};
	int robot_id = polarcontext_indexs[cur_ptr].first;
	int frame_id = polarcontext_indexs[cur_ptr].second;
	Eigen::VectorXf ringkey = polarcontext_ringkeys[robot_id].col(frame_id);; // current query ringkey
	auto polarcontext = polarcontexts[robot_id][frame_id]; // current feature

	// step 1: candidates from ringkey tree
    // 生成候选闭合帧的集合，用于检测当前点云帧与其他点云帧之间是否存在跨机器人的环回闭合
	// tree construction
	Eigen::MatrixXf new_polarcontext_ringkeys;//新的环形关键字矢量
	std::vector<int> new_indexes_maps;//新的索引映射
	std::vector<Eigen::MatrixXd> new_polarcontexts; //新的扇形关键字矩阵
    //如果当前点云帧所属机器人为当前节点机器人，将所有其他机器人的点云帧加入到候选帧的集合中；
	if(robot_id == id_)
	{
		for(int i = 0; i < robot_num_; i++)
		{
			if(i != id_ && indexes_maps[i].size() > 0)
			{
				int cur_row = new_polarcontext_ringkeys.cols();
				int add_row = indexes_maps[i].size();
				new_polarcontext_ringkeys.conservativeResize(pc_ring_num_, cur_row + add_row);
				new_polarcontext_ringkeys.block(0, cur_row, pc_ring_num_, add_row) = polarcontext_ringkeys[i].block(0, 0, pc_ring_num_, add_row);
				new_indexes_maps.insert(new_indexes_maps.end(), indexes_maps[i].begin(), indexes_maps[i].end());
				new_polarcontexts.insert(new_polarcontexts.end(), polarcontexts[i].begin(), polarcontexts[i].end());
			}
		}
	}
    // 否则只将当前机器人的点云帧加入到候选帧的集合中。
	else
	{
		if(indexes_maps[id_].size() > 0)
		{
			int size = indexes_maps[id_].size();
			new_polarcontext_ringkeys.conservativeResize(pc_ring_num_, size);
			new_polarcontext_ringkeys.block(0, 0, pc_ring_num_, size) = polarcontext_ringkeys[id_].block(0, 0, pc_ring_num_, size);
			new_indexes_maps.insert(new_indexes_maps.end(), indexes_maps[id_].begin(), indexes_maps[id_].end());
			new_polarcontexts.insert(new_polarcontexts.end(), polarcontexts[id_].begin(), polarcontexts[id_].end());
		}
	}

    // 用于检查生成的候选闭合帧集合的大小是否满足要求。
    // 如果候选闭合帧数量小于预设的数量 candidates_num_ + 1，
    // 说明生成的候选闭合帧数量不足，无法进行后续处理，因此函数直接返回原始结果 result
	if(new_indexes_maps.size() < candidates_num_ + 1)
	{
		return result;
	}


    // 使用 Nabo 库进行 K 近邻搜索
    // 根据新生成的环形关键字矢量 new_polarcontext_ringkeys 创建一个 KD 树，
    // 使用 Nabo::NNSearchF::createKDTreeLinearHeap 函数进行创建。
    // 其中，pc_ring_num_ 表示环形关键字矢量的行数，即扇形关键字数量。
	kdTree = Nabo::NNSearchF::createKDTreeLinearHeap(new_polarcontext_ringkeys, pc_ring_num_);


	// search n nearest neighbors
    //一个大小为 candidates_num_ 的整型向量 indice
    // 一个大小为 candidates_num_ 的浮点型向量 distance，用于存储搜索结果
	Eigen::VectorXi indice(candidates_num_);
	Eigen::VectorXf distance(candidates_num_);
	float min_distance = 10000000.0;
	int min_index = -1;
	int min_bias = 0;

	// knn search
    // 根据搜索结果计算最小距离和对应的闭合帧索引。
    // 具体操作是，遍历搜索结果，找到距离最小的闭合帧，即距离为 min_distance 的闭合帧。
    // 如果有多个距离相等的闭合帧，则选择具有最小索引偏移量的闭合帧。
    // 最终得到的 min_index 和 min_bias 分别表示距离最小的闭合帧的索引和索引偏移量
	kdTree->knn(ringkey, indice, distance, candidates_num_);

	// step 2: pairwise distance
    // 用于遍历 K 近邻搜索结果，计算每个候选闭合帧与当前点云帧之间的距离，并找到距离最小的闭合帧
    //使用 std::min 函数计算实际要遍历的搜索结果的数量，即 candidates_num_ 和 indice.size() 中的较小值
	for(int i = 0; i < std::min(candidates_num_, int(indice.size())); i++)
	{
        //判断其索引是否超出了新索引映射的范围，如果是，则跳过该搜索结果
		if(indice[i] >= new_indexes_maps.size())
		{
			continue;
		}

        // 根据搜索结果的索引 indice[i] 获取对应的候选闭合帧的扇形关键字矩阵 polarcontext_candidate
        // 并使用 distanceBtnScanContext 函数计算当前点云帧与该候选闭合帧之间的距离
		auto polarcontext_candidate = new_polarcontexts[indice[i]];
		std::pair<double, int> sc_dist_result = distanceBtnScanContext(polarcontext, polarcontext_candidate); 

		double candidateDis = sc_dist_result.first;
		int candidate_align = sc_dist_result.second;

        //判断计算得到的闭合帧距离是否小于 min_distance，
        // 如果是，则更新 min_distance、min_index 和 min_bias 的值，
        // 分别设置为当前的候选闭合帧距离、候选闭合帧的索引和对齐偏移量
		if(candidateDis < min_distance)
		{
			min_distance = candidateDis;
			min_index = new_indexes_maps[indice[i]];
			min_bias = candidate_align;
		}
	}

	// loop threshold check
    //用于根据计算得到的最小距离和对应的闭合帧索引，判断是否存在闭合回路
    // 判断计算得到的最小距离是否小于预设的距离阈值 distance_threshold_。
    // 如果是，则说明当前点云帧与最优闭合帧之间存在闭合回路，
    // 设置 result 的值为最优闭合帧的索引和对齐偏移量，并输出闭合回路信息。
    // 否则，说明当前点云帧与最优闭合帧之间不存在闭合回路，输出非闭合回路信息
	if(min_distance < distance_threshold_)
	{
		result.first = min_index;
		result.second = min_bias;
		ROS_DEBUG("\033[1;33m[SC Loop<%d>] btn %d-%d and %d-%d. Dis: %.2f.\033[0m", id_,
			polarcontext_indexs[cur_ptr].first, polarcontext_indexs[cur_ptr].second,
			polarcontext_indexs[min_index].first, polarcontext_indexs[min_index].second, min_distance);
            // [cur_ptr].first和[cur_ptr].second当前点云帧的扇形关键字矩阵的起始索引和终止索引
            // min_index 表示最优闭合帧的起始索引和终止索引，min_distance 表示当前点云帧和最优闭合帧之间的距离
	}
	else
	{
		ROS_DEBUG("\033[1;33m[SC Not loop<%d>] btn %d-%d and %d-%d. Dis: %.2f.\033[0m", id_,
			polarcontext_indexs[cur_ptr].first, polarcontext_indexs[cur_ptr].second,
			polarcontext_indexs[min_index].first, polarcontext_indexs[min_index].second, min_distance);
	}

    // 返回结果 result
    // result.first 表示最优闭合帧的索引，
    // result.second 表示需要将最优闭合帧顺时针旋转多少个扇形关键字矩阵，才能与当前点云帧对齐
	return result;
}

// 代码是 scan_context_descriptor 命名空间中的 getIndex 函数
// 获取给定关键字 key 对应的扇形关键字矩阵中的机器人编号和帧编号
// 并返回一个 std::pair<int8_t, int> 类型的结果对象，
// 其中第一个元素表示机器人编号，第二个元素表示帧编号
std::pair<int8_t, int> scan_context_descriptor::getIndex(
	const int key)
{
	return polarcontext_indexs[key];
}

// 代码是 scan_context_descriptor 命名空间中的 getSize 函数，
// 用于获取扇形关键字矩阵的大小，即点云帧的数量
// 函数接受一个整型变量 id，表示机器人的编号，缺省值为 -1，表示获取所有机器人的点云帧数量。
// 如果 id 不为 -1，则只获取指定机器人的点云帧数量。
int scan_context_descriptor::getSize(
	const int id = -1)
{
	if(id == -1)
	{
		return polarcontext_indexs.size();
	}
	else
	{
		return indexes_maps[id].size();
	}
}
